#ifndef _WF_TASK1_LISTENER_HPP_
#define _WF_TASK1_LISTENER_HPP_

#include <rclcpp/rclcpp.hpp>
#include <rclcpp_components/register_node_macro.hpp>
#include <sensor_msgs/msg/image.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <cv_bridge/cv_bridge.h>
#include <string>
#include <iostream>

#include <tf2/LinearMath/Quaternion.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2_eigen/tf2_eigen.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2_ros/static_transform_broadcaster.h>

#include <opencv2/opencv.hpp>
#include <opencv2/core.hpp>
#include "opencv2/imgproc.hpp"
#include "opencv2/calib3d.hpp"
#include <opencv2/highgui.hpp>

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/search/impl/search.hpp>
#include <pcl/range_image/range_image.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/common/common.h>
#include <pcl/common/transforms.h>
#include <pcl/registration/icp.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/filter.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/crop_box.h>
#include <pcl_conversions/pcl_conversions.h>

namespace wf 
{
    namespace listener 
    {
        using TransformStamped = geometry_msgs::msg::TransformStamped;
        using Image = sensor_msgs::msg::Image;
        using PointCloud2 = sensor_msgs::msg::PointCloud2;
        using PoseStamped = geometry_msgs::msg::PoseStamped;
        using PointStamped = geometry_msgs::msg::PointStamped;

        class ListenerNode : public rclcpp::Node 
        {
            private:
                int tot_img_ = -1, tot_pcd2_ = -1, tot_pose_ = -1;

                std::string source_frame_;
                std::string target_frame_;

                std::unique_ptr<tf2_ros::Buffer> p_tfBuffer_;

                PointStamped source_point_;
                PointStamped target_point_;

                std::shared_ptr<tf2_ros::TransformListener> tf_subsriber_;
                rclcpp::Subscription<Image>::SharedPtr image_subscriber_;
                rclcpp::Subscription<PointCloud2>::SharedPtr pcd2_subscriber_;
                rclcpp::Subscription<PoseStamped>::SharedPtr pose_subscriber_;

                void init();

                void onReceiveStaticTF(const TransformStamped::SharedPtr p_tf);
                void onReceiveImage(const Image::SharedPtr p_img);
                void onReceivePcd2(const PointCloud2::SharedPtr p_pcd2);
                void onReceivePose(const PoseStamped::SharedPtr p_pose);

            public:
                explicit ListenerNode(const rclcpp::NodeOptions &options);
        };
    }
}


#endif